%%GENERAL VARS
% Distance Units = Meters
% Angle Units = Radian
% Robot width = Robot width + 0.1 m
% Exploration time = 30 min
% Every position data fits the template = [x y theta]
% currentPose = [x y theta]
% worldMap =  [x1 y1 ; x2 y2]
% Max Kinect sight = 4 m
% Min Kinect sight = 0.8 m


%% StateMachine
%**************************************BERNARD
    % It's DA thing.
    % function [] = main()
    % 
    % Initialize
    % function [] = initSensors() %OK%
    % 
    % Returns true if Ros is exploring and false if it is coming back.
    % function [isExploring] = isExplorationMode()
    %
    
%% Kinect - Exploration
%**************************************OLIVIER
    %function [updatedWorldMap,updatedPose] = getCurrentLocationAndUpdate(worldMap,currentPose)
    
    %Returns value between [80,400] or inf if any other values (too close or too far).
    %function [distanceToObstacle] = assessObstacleDistance(worldMap,currentPose)
    %
    %

%% Kinect - Return mode
%**************************************OLIVIER
    % Acquired Data is not used to complement virtual map.
    % function [updatedPose] = getCurrentLocation(worldMap,currentPose)
%% Deplacement
%**************************************BERNARD
    % Send current pose and desired pose. Must be called after Gyro is
    % subscribed and is filling its buffer.
    % 
    % function [startTime, endTime, updatedPose] = goFromPoseToPose(currentPose, desiredPose)
    % 
    % Send current pose and desired pose. Must be called after Gyro is
    % subscribed and is filling its buffer. Does not take into account
    % angle. desiredLocation format  = [x y theta] where theta is
    % irrelevant
    % function [startTime, endTime, updatedPose] = goFromPoseToLocation(currentPose, desiredLocation)
    %
%% Construct Environment
%**************************************MARC-OLIVIER
    % 
    % 
    % 
    % Append currentPose to the positionList
    % function [updatedPositionList] =  dropBreadcrumb(positionList, currentWorldmap, currentPose)
%% Navigation
    % Merge data to correct estimatedPosition using sensors to get a
    % currentPosition (Kalman filter)
    %%%COMPLETE THIS FUNCTION PROTOTYPE%%%
%**************************************MARC-OLIVIER
    % function [updatedPositionList] =  adjustCurrentPose(estimatedPose, )
    %
%**************************************VINCENT
    % This function acquire the rotation angle from the Gyroscope
    % function [rotationAngle] =  getRotationAngle(movementStartTime, movementEndTime)
    %
    %
    %
    %